#include <FLOW_SENSOR_module/FLOW_SENSOR.hpp>

#ifdef DBG_TAG
#undef DBG_TAG
#endif
#ifdef DBG_LVL
#undef DBG_LVL
#endif

#define DBG_TAG     "flow"
#define DBG_LVL     DBG_LOG

extern "C"
{
#include <rtdbg.h>
}

using namespace std;

//#define PMW3901_DEBUG
//#define PMW3901_DEBUG_RAW

/******************************************************************************
 *
 * @brief:     PMW3901类实现
 *
 ******************************************************************************/
PMW3901::PMW3901(CHIP_SPI* spi, CHIP_OUTPUT_PIN *reset_pin, CHIP_INPUT_PIN* motion_pin, uint32_t filter_time)
{
    shift_combine = 0;
    if (!spi || !reset_pin || !motion_pin)
    {
        LOG_E("input pra is null");
        goto end;
    }
    if (spi->CHIP_SPI_read_status() != chip_spi_status_ok)
        goto end;
    if (reset_pin->CHIP_OUTPUT_read_init_status() != chip_pin_status_ok)
        goto end;
    if (motion_pin->CHIP_INPUT_PIN_read_init_status() != chip_pin_status_ok)
        goto end;

    this->spi = spi;
    this->reset_pin = reset_pin;
    this->motion_pin = motion_pin;
    this->filter_time = filter_time;
    this->now_time = 0;
    this->x_temp = 0;
    this->y_temp = 0;

    shift_lock = rt_mutex_create("pmw3901_lock", RT_WAITING_FOREVER);
    if (!shift_lock)
    {
        LOG_E("pmw3901 lock create fail");
        goto end;
    }

    init_flag = pmw3901_status_ok;
    LOG_I("pmw3901 create ok!");
    return;
    end: init_flag = pmw3901_status_fail;
    LOG_E("pmw3901 create fail!");
    return;

}
PMW3901::~PMW3901()
{

}

pmw3901_status PMW3901::PMW3901_init()
{
    uint8_t buff[2] = { };
    uint8_t retry_time = 3;
    int cmd_pause_time = 1;
    uint8_t skip_flag = 0;
    uint8_t C1, C2;

//    spi->CHIP_SPI_open_debug();

    if (init_flag != pmw3901_status_ok)
        goto end;
    rt_thread_mdelay(100);

    this->now_time = 0;
    this->x_temp = 0;
    this->y_temp = 0;

    { //power_up 上电

        reset_pin->CHIP_OUTPUT_PIN_write(chip_pin_level_LOW);
        rt_thread_mdelay(1000);
        reset_pin->CHIP_OUTPUT_PIN_write(chip_pin_level_HIGH);
        rt_thread_mdelay(1000);

        //read id
        spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_PRODUCT_ID, buff);
        rt_thread_mdelay(cmd_pause_time);
        if (buff[0] != 0x49)
        {
            LOG_E("read pmw3901 id 0x%.2X != 0x49 , connect error showed", buff[0]);
            goto end;
        }

        spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_MOTION, buff);
        rt_thread_mdelay(cmd_pause_time);
        spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_DELTA_X_L, buff);
        rt_thread_mdelay(cmd_pause_time);
        spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_DELTA_X_H, buff);
        rt_thread_mdelay(cmd_pause_time);
        spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_DELTA_Y_L, buff);
        rt_thread_mdelay(cmd_pause_time);
        spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_DELTA_Y_H, buff);
        rt_thread_mdelay(cmd_pause_time);
    }

    { //set_optimum_performance 性能优化
        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x55;
        buff[1] = 0x01;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x50;
        buff[1] = 0x07;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0E;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        while (retry_time-- && buff[1] != 0x08)
        {
            buff[0] = 0x80 | 0x43;
            buff[1] = 0x10;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);

            buff[0] = 0x47;
            buff[1] = 0x00;
            spi->CHIP_SPI_send_uint8_and_read_uint8(buff[0], &buff[1]);
            rt_thread_mdelay(cmd_pause_time);

        }
        if (buff[1] != 0x08)
            goto end;

        buff[0] = 0x67;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_and_read_uint8(buff[0], &buff[1]);
        rt_thread_mdelay(cmd_pause_time);
        if ((buff[1] & 0x80) == 0x80)
        {
            buff[0] = 0x80 | 0x48;
            buff[1] = 0x04;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);
        }
        else
        {
            buff[0] = 0x80 | 0x48;
            buff[1] = 0x02;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);
        }

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x51;
        buff[1] = 0x7B;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x50;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x55;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0E;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x73;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_and_read_uint8(buff[0], &buff[1]);
        rt_thread_mdelay(cmd_pause_time);
        if (buff[1])
        {
            LOG_I("pmw3901 0x73 read != 0x00 skip");
            skip_flag = 1;
        }
        else
        {
            LOG_I("pmw3901 0x73 read == 0x00 no skip");
            skip_flag = 0;
        }

        if (skip_flag == 0)
        {
            buff[0] = 0x70;
            buff[1] = 0x00;
            spi->CHIP_SPI_send_uint8_and_read_uint8(buff[0], &buff[1]);
            rt_thread_mdelay(cmd_pause_time);
            if (buff[1] <= 28)
                C1 = buff[1] + 14;
            else
                C1 = buff[1] + 11;
            if (C1 > 0x3F)
                C1 = 0x3F;

            buff[0] = 0x71;
            buff[1] = 0x00;
            spi->CHIP_SPI_send_uint8_and_read_uint8(buff[0], &buff[1]);
            rt_thread_mdelay(cmd_pause_time);
            C2 = buff[1] * 45 / 100;

            buff[0] = 0x80 | 0x7F;
            buff[1] = 0x00;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);

            buff[0] = 0x80 | 0x61;
            buff[1] = 0xAD;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);

            buff[0] = 0x80 | 0x51;
            buff[1] = 0x70;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);

            buff[0] = 0x80 | 0x7F;
            buff[1] = 0x0E;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);

            buff[0] = 0x80 | 0x70;
            buff[1] = C1;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);

            buff[0] = 0x80 | 0x71;
            buff[1] = C2;
            spi->CHIP_SPI_send_uint8_array(buff, 2);
            rt_thread_mdelay(cmd_pause_time);
        }

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x61;
        buff[1] = 0xAD;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x03;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x05;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x41;
        buff[1] = 0xB3;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x43;
        buff[1] = 0xF1;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x45;
        buff[1] = 0x14;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x5B;
        buff[1] = 0x32;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x5F;
        buff[1] = 0x34;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7B;
        buff[1] = 0x08;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x06;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x44;
        buff[1] = 0x1B;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x40;
        buff[1] = 0xBF;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4E;
        buff[1] = 0x3F;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x08;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x65;
        buff[1] = 0x20;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x6A;
        buff[1] = 0x18;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x09;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4F;
        buff[1] = 0xAF;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x5F;
        buff[1] = 0x40;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x48;
        buff[1] = 0x80;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x49;
        buff[1] = 0x80;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x57;
        buff[1] = 0x77;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x60;
        buff[1] = 0x78;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x61;
        buff[1] = 0x78;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x62;
        buff[1] = 0x08;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x63;
        buff[1] = 0x50;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0A;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x45;
        buff[1] = 0x60;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4D;
        buff[1] = 0x11;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x55;
        buff[1] = 0x80;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x74;
        buff[1] = 0x1F;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x75;
        buff[1] = 0x1F;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4A;
        buff[1] = 0x78;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4B;
        buff[1] = 0x78;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x44;
        buff[1] = 0x08;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x64;
        buff[1] = 0xFF;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x65;
        buff[1] = 0x1F;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x14;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x65;
        buff[1] = 0x67;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x66;
        buff[1] = 0x08;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x63;
        buff[1] = 0x70;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x15;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x48;
        buff[1] = 0x48;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x07;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x41;
        buff[1] = 0x0D;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x43;
        buff[1] = 0x14;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4B;
        buff[1] = 0x0E;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x45;
        buff[1] = 0x0F;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x44;
        buff[1] = 0x42;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4C;
        buff[1] = 0x80;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x10;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x5B;
        buff[1] = 0x02;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x07;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x41;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x70;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        rt_thread_mdelay(10);

        buff[0] = 0x80 | 0x32;
        buff[1] = 0x44;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x07;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x40;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x06;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x62;
        buff[1] = 0xF0;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x63;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0D;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x48;
        buff[1] = 0xC0;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x6F;
        buff[1] = 0xD5;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x5B;
        buff[1] = 0xA0;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x4E;
        buff[1] = 0xA8;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x5A;
        buff[1] = 0x50;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x80;
        spi->CHIP_SPI_send_uint8_array(buff, 2);
        rt_thread_mdelay(cmd_pause_time);

    }
    LOG_I("pmw3901 optimum performance finish!");
    init_flag = pmw3901_status_ok;
    return pmw3901_status_ok;
    end:
    LOG_E("pmw3901 optimum performance fail!");
    init_flag = pmw3901_status_fail;
    return pmw3901_status_fail;
}

pmw3901_status PMW3901::PMW3901_burst_read_motion(Ppmw3901_motion_data_st p_motion)
{
    uint8_t addr = PMW3901_REG_MOTION_BURST;
    uint8_t buff[12] = { };
    if (init_flag != pmw3901_status_ok)
        goto end;
    if (!p_motion)
    {
        LOG_E("pmw3901 burst read can't input null ptr");
        goto end;
    }

    spi->CHIP_SPI_send_uint8_array_and_read_uint8_array(&addr, 1, buff, 12);
    if ((buff[0] & (1 << 7)) != 0)
    {
        if ((buff[6] < 0x19) || (buff[10] == 0x1F))
        {
            LOG_W("pmw3901 detect move but data can't trust");
            p_motion->flag = pmw3901_status_fail; /* success return 0 */
        }
        p_motion->delta_x = (int16_t) (((uint16_t) buff[3] << 8) | buff[2]);
        p_motion->delta_y = (int16_t) (((uint16_t) buff[5] << 8) | buff[4]);
        p_motion->flag = pmw3901_status_ok;
//            LOG_D("move x <%d> y<%d>",p_motion->delta_x,p_motion->delta_y);
    }
    else
    {
        p_motion->flag = pmw3901_status_fail;
    }

//    rt_kprintf("\r\n");
//    for(i = 0 ; i < 12;i++)
//    {
//        rt_kprintf("%d :<0x%.2X> ",i,buff[i]);
//    }
//    rt_kprintf("\r\n");
//    LOG_I("pmw3901 brust read motion finish!");
    return pmw3901_status_ok;
    end:
    LOG_E("pmw3901 brust read motion fail!");
    return pmw3901_status_fail;
}

pmw3901_status PMW3901::PWM3901_read_out_shift_and_reduce(uint32_t *out_buff)
{
    uint32_t temp;
    if (init_flag != pmw3901_status_ok)
        goto end;
    if (!out_buff)

    {
        LOG_E("out buff is null");
        goto end;
    }

    rt_mutex_take(shift_lock, RT_WAITING_FOREVER);

    temp = shift_combine / 10;
#ifdef PMW3901_DEBUG
    LOG_D("now shift <%f> wave count <%u> after send shift <%f>",
            shift_combine,
            temp,
            shift_combine - temp * 10);
#endif
    shift_combine = shift_combine - temp * 10;

    rt_mutex_release(shift_lock);
    *out_buff = temp;

    return pmw3901_status_ok;
    end: rt_mutex_release(shift_lock);
    return pmw3901_status_fail;
}
pmw3901_status PMW3901::PWM3901_count_shift_with_height(double height)
{
    pmw3901_motion_data_st motion;
    double resolution_cpi;
    double x, y;

    if (init_flag != pmw3901_status_ok)
        goto end;

    if (PMW3901_burst_read_motion(&motion) != pmw3901_status_ok)
        goto end;
    if (motion.flag != pmw3901_status_ok)
        goto cant_trust;

    //放置在此处以便于清除错误数据后再退出
    if (height == 0)
        goto end;
    height -= 10;

    resolution_cpi = 11.914f / (height * 25.4)* 1000 ;

//    LOG_D("cx %d, cy %d",motion.delta_x,motion.delta_y);
    x = motion.delta_x / resolution_cpi; //mm
    y = motion.delta_y / resolution_cpi; //mm

    x_temp += x;
    y_temp += y;
#ifdef PMW3901_DEBUG_RAW
    LOG_D(" motion x %d y %d", motion.delta_x, motion.delta_y);
    LOG_D(" x %lf y %lf x_t %lf y_t %lf", x, y, x_temp, y_temp);
#endif
    if (now_time < filter_time)
        now_time++;
    else
    {
        rt_mutex_take(shift_lock, RT_WAITING_FOREVER);

        shift_combine += sqrt(x_temp * x_temp + y_temp * y_temp);
        y_temp = x_temp = 0;
        now_time = 0;

#ifdef PMW3901_DEBUG_RAW
        LOG_D("now height %lf shift count %lf filter times <%u>", height, shift_combine, now_time);
#endif
        rt_mutex_release(shift_lock);
    }

    return pmw3901_status_ok;
    cant_trust: return pmw3901_status_fail;
    end:
    LOG_E("pmw3901 count shift fail!");
    return pmw3901_status_fail;

}

pmw3901_status PMW3901::PWM3901_read_id()
{
    uint8_t buff[2] = { };
    //read id
    spi->CHIP_SPI_send_uint8_and_read_uint8(PMW3901_REG_PRODUCT_ID, buff);
    rt_thread_mdelay(1);
    if (buff[0] != 0x49)
    {
        LOG_E("read pmw3901 id 0x%.2X != 0x49 , connect error showed", buff[0]);
        goto end;
    }

    return pmw3901_status_ok;
    end: return pmw3901_status_fail;
}

